#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from lib.MiniBotControlLib import MiniBotControlLib
from RobotManipulator.ManipulatorRobot import ManipulatorRobot
from RobotManipulator.ManipulatorActions import ManipulatorActions
import keyboard
import time

robotlib = BwRobotLib()
manipulator = ManipulatorRobot(robotlib)
#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55000)
#robotlib.connectRobot("10.10.100.254",55000)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)

#配置操作臂
manipulator.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#操作臂
#-----------------------------------------------------------------------------


#运行
def runDemo():
  manipulator.pickUpSample()
  time.sleep(2)
  manipulator.putDownSample()

while(1):
  print("Enter any key to run ....")
  input("")
  runDemo()

